import javax.vecmath.*;

public class CollisionHandler {
	private static final float COEFFICIENT_OF_RESTITUTION = 1.0f; //0.9
	private static final float COEFFICIENT_OF_FRICTION = 1.0f;
	
	public static void checkAndResolveCollision(PhysicsObject a, PhysicsObject b) {
		CollisionInfo ci = getCollisionInfo(a, b);
		if (ci == null)
			return;
		
		

		// Vector from the center of mass of object a to the collision point
		Vector2f r_ap = new Vector2f();
		r_ap.scaleAdd(-1, a.getGlobalCenterOfMass(), ci.position);
		// Vector from the center of mass of object b to the collision point
		Vector2f r_bp = new Vector2f();
		r_bp.scaleAdd(-1, b.getGlobalCenterOfMass(), ci.position);
		
		// Velocity of object a at the point of collision
		Vector2f v_ap1 = new Vector2f();
		v_ap1.x = a.velocity.x - a.angularVelocity * r_ap.y;
		v_ap1.y = a.velocity.y + a.angularVelocity * r_ap.x;
		
		// Velocity of object b at the point of collision
		Vector2f v_bp1 = new Vector2f();
		v_bp1.x = b.velocity.x - b.angularVelocity * r_bp.y;
		v_bp1.y = b.velocity.y + b.angularVelocity * r_bp.x;
		
		// The collision impulse
		Vector2f v_ab1 = new Vector2f();
		v_ab1.scaleAdd(-1, v_bp1, v_ap1);
		float tmpA = r_ap.x * ci.normal.y - r_ap.y * ci.normal.x;
		float tmpB = r_bp.x * ci.normal.y - r_bp.y * ci.normal.x;
		float j = -(1 + COEFFICIENT_OF_RESTITUTION) * v_ab1.dot(ci.normal) / (1 / a.mass + 1 / b.mass + tmpA * tmpA / a.momentOfInertia + tmpB * tmpB / b.momentOfInertia);
		
		
		// update with friction
		
		
		
//
//		// Update object a's velocity
		a.velocity.scaleAdd(j / a.mass, ci.normal, a.velocity);
//		// Update object b's velocity
		b.velocity.scaleAdd(-j / b.mass, ci.normal, b.velocity);
//		// Update object a's angular velocity
		a.angularVelocity += j * (r_ap.x * ci.normal.y - r_ap.y * ci.normal.x) / a.momentOfInertia;
//		// Update object b's angular velocity
		b.angularVelocity -= j * (r_bp.x * ci.normal.y - r_bp.y * ci.normal.x) / b.momentOfInertia;
//		
		
		// Remove object overlap
		a.position.scaleAdd(-ci.depth / (a.mass * (1 / a.mass + 1 / b.mass)), ci.normal, a.position);
		b.position.scaleAdd(ci.depth / (b.mass * (1 / a.mass + 1 / b.mass)), ci.normal, b.position);
		
		
		
		//friction stuff
		/*
		if(!(a instanceof HalfSpace)) {
			Vector2f afinal = new Vector2f();
			float angvel = a.angularVelocity;
			if(!(b instanceof HalfSpace))
				angvel += b.angularVelocity;
			
			float numerator = (1-a.angularVelocity * COEFFICIENT_OF_RESTITUTION) * a.velocity.x;
			numerator += a.momentOfInertia * (1 + COEFFICIENT_OF_RESTITUTION) * getWidth(a) * angvel;
			
			float denominator = a.momentOfInertia + 1;
			
			afinal.x = numerator/denominator;
			afinal.y = -COEFFICIENT_OF_RESTITUTION * a.velocity.y;
			if(!Float.isNaN(afinal.x) && !Float.isInfinite(afinal.x)
					&& !Float.isNaN(afinal.y) && !Float.isInfinite(afinal.y)) {
				a.velocity = afinal;
			}
		}
		
		if(!(b instanceof HalfSpace)) {
			Vector2f afinal = new Vector2f();
			float angvel = b.angularVelocity;
			if(!(a instanceof HalfSpace))
				angvel += a.angularVelocity;
			
			float numerator = (1-b.angularVelocity * COEFFICIENT_OF_RESTITUTION) * b.velocity.x;
			numerator += b.momentOfInertia * (1 + COEFFICIENT_OF_RESTITUTION) * getWidth(b) * angvel;
			
			float denominator = b.momentOfInertia + 1;
			
			afinal.x = numerator/denominator;
			afinal.y = -COEFFICIENT_OF_RESTITUTION * b.velocity.y;
			if(!Float.isNaN(afinal.x) && !Float.isInfinite(afinal.x)
					&& !Float.isNaN(afinal.y) && !Float.isInfinite(afinal.y)) {
				
				
				System.out.println(afinal.toString());
				
				b.velocity = afinal;
			}
		}
		//*/
		
		/*
		//friction stuff
		if(!Float.isInfinite(a.momentOfInertia) && !Float.isInfinite(b.momentOfInertia)) {
			Vector2f perp_a = new Vector2f(-ci.normal.y, ci.normal.x);
			Vector2f neg_norm = new Vector2f(-ci.normal.x, -ci.normal.y);
			
			//perp_a.normalize();
			
			float tva = a.angularVelocity * r_ap.length();
			float tvb = b.angularVelocity * r_bp.length();
			
			Vector2f tang_a = new Vector2f(perp_a.x * tva, perp_a.y * tva);
			Vector2f tang_b = new Vector2f(perp_a.x * tvb, perp_a.y * tvb);
			
			System.out.println("perp_a: " + perp_a.toString() + ", " + perp_a.length());
			System.out.println("angular velocity: " + a.angularVelocity);
			System.out.println("distance: " + r_ap.toString() + ", " + r_ap.length());
			System.out.println("tangential velocity: " + tva);
			System.out.println("tang_a: " + tang_a.toString() + ", " + tang_a.length());
			System.out.println("j: " + j);
			System.out.println("old velocity: " + a.velocity.toString() + ", " + a.velocity.length());
			System.out.println("moment of inertia a: " + a.momentOfInertia);
			
			
			float oldspeeda = a.velocity.length();
			float oldspeedb = b.velocity.length();
			
			Vector2f old_vel_a = a.velocity;
			/*
			a.velocity.x -= tang_a.x;
			a.velocity.y -= tang_a.y;
			
			b.velocity.x -= tang_b.x;
			b.velocity.y -= tang_b.y;
			
			a.velocity.normalize();
			a.velocity.x *= oldspeeda;
			a.velocity.y *= oldspeeda;
			
			b.velocity.normalize();
			b.velocity.x *= oldspeedb;
			b.velocity.y *= oldspeedb;
			
			
			double theta = Math.acos((old_vel_a.dot(neg_norm))/(old_vel_a.length() * ci.normal.length()));
			double phi = Math.acos(old_vel_a.dot(a.velocity)/(old_vel_a.length() * a.velocity.length()));
			
			float ratio = 0;
			if(theta != 0.0)
				ratio = (float)phi/(float)theta;
			
			if(theta != 0.0 && !Float.isInfinite(ratio) && !Float.isNaN(ratio))
				a.angularVelocity = a.angularVelocity - (ratio*a.angularVelocity);
			
			System.out.println("new velocity: " + a.velocity.toString() + ", " + a.velocity.length());
			System.out.println("");
			//*/
		//}
		
		a.clearCaches();
		b.clearCaches();
	}
//	
//	private static float crossProduct(Vector2f u, Vector2f v) {
//		return u.x * v.y - u.y * v.x;
//	}
//	
//	private static Vector2f vectorMultiply(float s, Vector2f v) {
//		return new Vector2f(s * v.x, s * v.y);
//	}
	
	private static CollisionInfo getCollisionInfo(PhysicsObject a, PhysicsObject b) {
		if (a == b)
			return null;
		
		CollisionInfo ci = null;
		if (a instanceof HalfSpace) {
			if (b instanceof Circle)
				ci = getCollision((HalfSpace)a, (Circle)b);
			else if (b instanceof Polygon)
				ci = getCollision((HalfSpace)a, (Polygon)b);
		} else if (a instanceof Circle) {
			if (b instanceof Circle)
				ci = getCollision((Circle)a, (Circle)b);
			else if(b instanceof Polygon)
				ci = getCollision((Circle)a, (Polygon)b);
		} else if (a instanceof Polygon) {
			if (b instanceof Polygon)
				ci = getCollision((Polygon)a, (Polygon)b);
			else if(b instanceof Circle)
				ci = getCollision((Polygon)a, (Circle)b);
		}
		return ci;
	}
	
	private static float vectorDistance(Vector2f a, Vector2f b) {
		float xx = a.x - b.x;
		float yy = a.y - b.y;
		
		return (float)Math.sqrt(xx*xx + yy*yy);
	}
	
	/*
	private static Vector2f pointBetween(Vector2f a, Vector2f b) {
		Vector2f blah = new Vector2f();
		blah.x = (a.x + b.x)/2;
		blah.y = (a.y + b.y)/2;
		return blah;
	}
	*/
	
	private static CollisionInfo getCollision(Polygon a, Circle b) {
		/*
		//this code didn't work so well.. 
		CollisionInfo ci = getCollision(b,a);
		if(null == ci)
			return null;
		ci.normal.scaleAdd(-1, new Vector2f(0,0));
		return ci;
		//*/
		
		
		//normal of collision is of the triangle
		
		Vector2f[] tri_verts = a.getVertices();
		Vector2f[] tri_norms = a.getNormals();
		
		//first check triangle corners
		for(int i = 0 ; i < tri_verts.length ; i++) {
			float dist = vectorDistance(tri_verts[i],b.position);
			if(dist < b.radius) {
				CollisionInfo info = new CollisionInfo();
				info.position = tri_verts[i];
				//info.normal = tri_norms[i];
				//int index = (0 == i) ? tri_norms.length-1 : i-1;
				//info.normal.add(tri_norms[index]);
				
				//info.normal = new Vector2f(tri_verts[i].x - b.position.x, 
				//		tri_verts[i].y - b.position.y);
				
				info.normal = new Vector2f();
				info.normal.scaleAdd(-1, tri_verts[i], b.position);
				info.normal.normalize();
				info.depth = b.radius - dist;
			}
		}
		
		
		//check triangle edges
		//then check circle edge with triangle edges
		Vector2f C = b.position;
		//Vector2f[] tri_norms = b.getNormals();
		for(int i = 0 ; i < tri_verts.length ; i++) {
			//System.out.println(i + ": " + tri_verts[i].toString());
			
			Vector2f R = tri_verts[i];
			Vector2f T = tri_verts[(i+1)%tri_verts.length];		
			Vector2f RT = new Vector2f(T.x - R.x, T.y - R.y);
			Vector2f RC = new Vector2f(C.x - R.x, C.y - R.y);

			float proj = (RT.dot(RC)) / RT.length();
			Vector2f B = new Vector2f(RT);
			B.scale(proj/RT.length());
			if(B.length() > RT.length())
				continue;
			
			B.add(R);
			
			//if B isn't within the rectangle made by R and T, skip
			if((B.x > R.x && B.x > T.x) || (B.x < R.x && B.x < T.x))
				continue;
			if((B.y > R.y && B.y > T.y) || (B.y < R.y && B.y < T.y))
				continue;

			if(vectorDistance(B,C) < b.radius && (RC.length() < RT.length())) {
				CollisionInfo info = new CollisionInfo();
				//get collision position
				info.position = B;
				//get collision normal
				//info.normal = new Vector2f(C.x - B.x, C.y - B.y);
				//info.normal.normalize();
				info.normal = tri_norms[i];
				//get depth
				info.depth = b.radius - vectorDistance(B,C);
				return info;
			}
		}
		
		
		return null;
		
		//*/
	}
	
	
	private static CollisionInfo getCollision(Circle a, Polygon b) {
		Vector2f[] tri_verts = b.getVertices();
		Point2f circle_center = new Point2f(a.position);
		
		//first check triangle corners
		for(int i = 0 ; i < tri_verts.length ; i++) {
			Point2f tri_corner = new Point2f(tri_verts[i]);
			float dist = tri_corner.distance(circle_center);
			if(dist < a.radius) {
				CollisionInfo info = new CollisionInfo();
				//get collision position
				info.position = new Vector2f(tri_verts[i]);
				//get collision normal
				//info.normal = new Vector2f(tri_verts[i].x - circle_center.x, 
				//		tri_verts[i].y - circle_center.y);
				//info.normal.x = -info.normal.x;
				//info.normal.y = -info.normal.y;
				info.normal = new Vector2f();
				info.normal.scaleAdd(1, tri_verts[i], a.position);
				info.normal.normalize();
				//info.normal = new Vector2f(b.position.x - a.position.x, b.position.y - a.position.y);
				//get depth
				info.depth = a.radius - dist;
				return info;
			}
		}
		
		//then check circle edge with triangle edges
		Vector2f C = a.position;
		//Vector2f[] tri_norms = b.getNormals();
		for(int i = 0 ; i < tri_verts.length ; i++) {
			//System.out.println(i + ": " + tri_verts[i].toString());
			
			Vector2f R = tri_verts[i];
			Vector2f T = tri_verts[(i+1)%tri_verts.length];
			//System.out.println("R: " + R.toString() + "\nT: " + T.toString());
			//System.out.println("C: " + C.toString());
			
			Vector2f RT = new Vector2f(T.x - R.x, T.y - R.y);
			Vector2f RC = new Vector2f(C.x - R.x, C.y - R.y);
			
			
			//System.out.println("RT (" + i + "," + ((i+1)%tri_verts.length) + "): " + RT.toString());
			//System.out.println("RC: " + RC.toString());
			
			
			float proj = (RT.dot(RC)) / RT.length();
			Vector2f B = new Vector2f(RT);
			B.scale(proj/RT.length());
			if(B.length() > RT.length())
				continue;
			
			B.add(R);
			
			if((B.x > R.x && B.x > T.x) || (B.x < R.x && B.x < T.x))
				continue;
			if((B.y > R.y && B.y > T.y) || (B.y < R.y && B.y < T.y))
				continue;
			
			//System.out.println("proj: " + proj);
			//System.out.println("B: " + B.toString());
			//System.out.println("d: " + vectorDistance(B,C));
			//System.out.println("r: " + a.radius + "\n");
			
			if(vectorDistance(B,C) < a.radius && (RC.length() < RT.length())) {
				CollisionInfo info = new CollisionInfo();
				//get collision position
				info.position = B;
				//get collision normal
				//info.normal = new Vector2f(C.x - B.x, C.y - B.y);
				info.normal = new Vector2f(B.x - C.x, B.y - C.y); //new
				info.normal.normalize();
				//get depth
				info.depth = a.radius - vectorDistance(B,C);
				return info;
			}
		}
		
		
		return null;
	}

	private static CollisionInfo getCollision(HalfSpace a, Circle b) {
		float distance = a.normal.dot(b.position) - a.intercept - b.radius;
		if (distance < 0) {
			CollisionInfo ci = new CollisionInfo();
			ci.normal = a.normal;
			ci.depth = -distance;
			ci.position = new Vector2f();
			ci.position.scaleAdd(-(b.radius - ci.depth), ci.normal, b.position);
			return ci;
		}
		return null;
	}
	
	private static CollisionInfo getCollision(HalfSpace a, Polygon b) {
		Vector2f[] vertices = b.getVertices();
		float[] distances = new float[vertices.length];
		
		for (int i = 0; i < vertices.length; i++)
			distances[i] = a.normal.dot(vertices[i]) - a.intercept;
		
		int minIndex = 0;
		for (int i = 1; i < distances.length; i++)
			if (distances[i] < distances[minIndex])
				minIndex = i;
		if (distances[minIndex] >= 0)
			return null;
		
		CollisionInfo ci = new CollisionInfo();
		ci.depth = -distances[minIndex];
		ci.normal = a.normal;
		ci.position = new Vector2f(vertices[minIndex]);
		ci.position.scaleAdd(ci.depth, ci.normal, ci.position);
		return ci;
	}
	
	private static CollisionInfo getCollision(Circle a, Circle b) {
		Vector2f n = new Vector2f();
		n.scaleAdd(-1, a.position, b.position);
		float distance = n.length() - a.radius - b.radius;
		if (distance < 0) {
			CollisionInfo ci = new CollisionInfo();
			n.normalize();
			ci.normal = n;
			ci.depth = -distance;
			ci.position = new Vector2f();
			ci.position.scaleAdd(a.radius - ci.depth / 2, ci.normal, a.position);
			return ci;
		}
		return null;
	}
	
	private static CollisionInfo getCollision(Polygon a, Polygon b) {
		Vector2f[] verticesA = a.getVertices();
		Vector2f[] normalsA = a.getNormals();
		Vector2f[] verticesB = b.getVertices();
		Vector2f[] normalsB = b.getNormals();
		float[][] distanceFromA = new float[verticesA.length][verticesB.length];
		float[][] distanceFromB = new float[verticesB.length][verticesA.length];
		int[] indexMinDistanceFromA = new int[verticesA.length];
		int[] indexMinDistanceFromB = new int[verticesB.length];
		
		for (int i = 0; i < verticesA.length; i++) {
			for (int j = 0; j < verticesB.length; j++) {
				Vector2f tmp = new Vector2f();
				tmp.scaleAdd(-1, verticesA[i], verticesB[j]);
				distanceFromA[i][j] = tmp.dot(normalsA[i]);
				if (distanceFromA[i][j] < distanceFromA[i][indexMinDistanceFromA[i]])
					indexMinDistanceFromA[i] = j;
			}
			if (distanceFromA[i][indexMinDistanceFromA[i]] >= 0)
				return null;
		}
		for (int i = 0; i < verticesB.length; i++) {
			for (int j = 0; j < verticesA.length; j++) {
				Vector2f tmp = new Vector2f(verticesA[j]);
				tmp.scaleAdd(-1, verticesB[i], verticesA[j]);
				distanceFromB[i][j] = tmp.dot(normalsB[i]);
				if (distanceFromB[i][j] < distanceFromB[i][indexMinDistanceFromB[i]])
					indexMinDistanceFromB[i] = j;
			}
			if (distanceFromB[i][indexMinDistanceFromB[i]] >= 0)
				return null;
		}
		
		int indexMaxDistanceFromA = 0;
		for (int i = 1; i < verticesA.length; i++)
			if (distanceFromA[i][indexMinDistanceFromA[i]] > distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]])
				indexMaxDistanceFromA = i;
		int indexMaxDistanceFromB = 0;
		for (int i = 1; i < verticesB.length; i++)
			if (distanceFromB[i][indexMinDistanceFromB[i]] > distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]])
				indexMaxDistanceFromB = i;
		
		CollisionInfo ci = new CollisionInfo();
		if (distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]] > distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]]) {
			ci.depth = -distanceFromA[indexMaxDistanceFromA][indexMinDistanceFromA[indexMaxDistanceFromA]];
			ci.normal = new Vector2f(normalsA[indexMaxDistanceFromA]);
			ci.position = new Vector2f(verticesB[indexMinDistanceFromA[indexMaxDistanceFromA]]);
			ci.position.scaleAdd(-ci.depth, ci.normal, ci.position);
		} else {
			ci.depth = -distanceFromB[indexMaxDistanceFromB][indexMinDistanceFromB[indexMaxDistanceFromB]];
			ci.normal = new Vector2f(normalsB[indexMaxDistanceFromB]);
			ci.normal.scale(-1);
			ci.position = new Vector2f(verticesA[indexMinDistanceFromB[indexMaxDistanceFromB]]);
//			ci.position.scaleAdd(ci.depth, ci.normal, ci.position);
		}
		return ci;
	}
}
